import numpy as np

from Beisaier_path_planning import Beisaier_path_planning_function
from calculate_line_angle import calculate_line_angle
from dubins import dubins_path_planning
from is_all_points_inside_poly import is_all_points_inside_poly
from move_all_points import move_all_points


def TaoHang_turning_Transfer(TaoHang_start_point, TaoHang_start_yaw, TaoHang_end_point, TaoHang_end_yaw,
                    curvature, TRACTOR_WIDTH, forward_interval, boundary_poly):
    FLAG = False

    Planed_OR_not = False
    local_path_x = []
    local_path_y = []
    path_yaw = []
    lengths = []

    current_poly = boundary_poly.buffer(-TRACTOR_WIDTH/2, join_style=2, mitre_limit=5.0)

    Temp_local_start_x = TaoHang_start_point[0]
    Temp_local_start_y = TaoHang_start_point[1]
    start_yaw = TaoHang_start_yaw
    end_x = TaoHang_end_point[0]
    end_y = TaoHang_end_point[1]
    end_yaw = TaoHang_end_yaw

    # 向前迭代试验
    step_length = 3
    i = 1
    while step_length*i < 300:
        Planed_OR_not, local_path_x, local_path_y, path_yaw, lengths = Beisaier_path_planning_function(Temp_local_start_x, Temp_local_start_y, start_yaw, end_x,
                                                                                                       end_y, end_yaw, curvature, forward_interval)
        if Planed_OR_not:  # 如果基于现在的起点和终点规划出来了贝塞尔路线
            FLAG = True
            local_start_x = Temp_local_start_x
            local_start_y = Temp_local_start_y
            break
        else:  # 如果基于现在的起点和终点无法规划贝塞尔路线
            i = i + 1
            Temp_local_start_x = Temp_local_start_x + step_length * np.cos(start_yaw)
            Temp_local_start_y = Temp_local_start_y + step_length * np.sin(start_yaw)

    if not FLAG:
        Temp_local_start_x = TaoHang_start_point[0]
        Temp_local_start_y = TaoHang_start_point[1]
        step_length = 3
        i = 1
        # 如果向前迭代找不到，则向后迭代试验
        while step_length * i < 300:
            Planed_OR_not, local_path_x, local_path_y, path_yaw, lengths = Beisaier_path_planning_function(
                Temp_local_start_x, Temp_local_start_y, start_yaw, end_x,
                end_y, end_yaw, curvature, forward_interval)
            if Planed_OR_not:  # 如果基于现在的起点和终点规划出来了贝塞尔路线
                FLAG = True
                local_start_x = Temp_local_start_x
                local_start_y = Temp_local_start_y
                break
            else:  # 如果基于现在的起点和终点无法规划贝塞尔路线
                i = i + 1
                Temp_local_start_x = Temp_local_start_x - step_length * np.cos(start_yaw)
                Temp_local_start_y = Temp_local_start_y - step_length * np.sin(start_yaw)

    if FLAG:
        Planed_OR_not, local_path_x, local_path_y, path_yaw, lengths = Beisaier_path_planning_function(local_start_x, local_start_y, start_yaw, end_x, end_y, end_yaw, curvature, forward_interval)

    new_found_start_point = [local_start_x, local_start_y]

    return new_found_start_point, Planed_OR_not, local_path_x, local_path_y, path_yaw, lengths



def TaoHang_turning(TaoHang_start_point, TaoHang_start_yaw, TaoHang_end_point, TaoHang_end_yaw,
                    curvature, TRACTOR_WIDTH, forward_interval, boundary_poly):
    FLAG = False
    Planed_OR_not = False
    local_path_x = []
    local_path_y = []
    path_yaw = []
    lengths = []

    current_poly = boundary_poly.buffer(-TRACTOR_WIDTH/2, join_style=2, mitre_limit=5.0)

    Temp_local_start_x = TaoHang_start_point[0]
    Temp_local_start_y = TaoHang_start_point[1]
    start_yaw = TaoHang_start_yaw
    end_x = TaoHang_end_point[0]
    end_y = TaoHang_end_point[1]
    end_yaw = TaoHang_end_yaw

    # 向前迭代试验
    step_length = 3
    i = 1
    while step_length*i < 300:
        Planed_OR_not, local_path_x, local_path_y, path_yaw, lengths = Beisaier_path_planning_function(Temp_local_start_x, Temp_local_start_y, start_yaw, end_x,
                                                                                                       end_y, end_yaw, curvature, forward_interval)
        if Planed_OR_not:  # 如果基于现在的起点和终点规划出来了贝塞尔路线
            if is_all_points_inside_poly(local_path_x, local_path_y, current_poly):  # 如果路径点都在多边形中
                FLAG = True
                local_start_x = Temp_local_start_x
                local_start_y = Temp_local_start_y
                break
            else:  # 如果路径跑到了多边形外部
                i = i + 1
                Temp_local_start_x = Temp_local_start_x + step_length*np.cos(start_yaw)
                Temp_local_start_y = Temp_local_start_y + step_length*np.sin(start_yaw)
        else:  # 如果基于现在的起点和终点无法规划贝塞尔路线
            i = i + 1
            Temp_local_start_x = Temp_local_start_x + step_length * np.cos(start_yaw)
            Temp_local_start_y = Temp_local_start_y + step_length * np.sin(start_yaw)

    if not FLAG:
        Temp_local_start_x = TaoHang_start_point[0]
        Temp_local_start_y = TaoHang_start_point[1]
        step_length = 3
        i = 1
        # 如果向前迭代找不到，则向后迭代试验
        while step_length * i < 300:
            Planed_OR_not, local_path_x, local_path_y, path_yaw, lengths = Beisaier_path_planning_function(
                Temp_local_start_x, Temp_local_start_y, start_yaw, end_x,
                end_y, end_yaw, curvature, forward_interval)
            if Planed_OR_not:  # 如果基于现在的起点和终点规划出来了贝塞尔路线
                if is_all_points_inside_poly(local_path_x, local_path_y, current_poly):  # 如果路径点都在多边形中
                    FLAG = True
                    local_start_x = Temp_local_start_x
                    local_start_y = Temp_local_start_y
                    break
                else:  # 如果路径跑到了多边形外部
                    i = i + 1
                    Temp_local_start_x = Temp_local_start_x - step_length * np.cos(start_yaw)
                    Temp_local_start_y = Temp_local_start_y - step_length * np.sin(start_yaw)
            else:  # 如果基于现在的起点和终点无法规划贝塞尔路线
                i = i + 1
                Temp_local_start_x = Temp_local_start_x - step_length * np.cos(start_yaw)
                Temp_local_start_y = Temp_local_start_y - step_length * np.sin(start_yaw)

    if FLAG:
        Planed_OR_not, local_path_x, local_path_y, path_yaw, lengths = Beisaier_path_planning_function(local_start_x, local_start_y, start_yaw, end_x, end_y, end_yaw, curvature, forward_interval)

    new_found_start_point = [local_start_x, local_start_y]

    return new_found_start_point, Planed_OR_not, local_path_x, local_path_y, path_yaw, lengths




    """
    extra_width = 0.02 # 预留边界余量
    local_field_boundary_poly_buffered = local_field_boundary_poly.buffer( -(TRACTOR_WIDTH/2+extra_width), join_style=2, mitre_limit=5.0)


    # 第一条边界有2个点
    if len(local_intersection_points_path_0)==2:
        length_0 = np.sqrt( abs(local_intersection_points_path_0[0][0]-second_point[0])**2 + abs(local_intersection_points_path_0[0][1]-second_point[1])**2 )
        length_1 = np.sqrt( abs(local_intersection_points_path_0[1][0]-second_point[0])**2 + abs(local_intersection_points_path_0[1][1]-second_point[1])**2 )

        if length_0<length_1:
            third_point = local_intersection_points_path_0[0]
            fourth_point = local_intersection_points_path_0[1]
        else:
            third_point = local_intersection_points_path_0[1]
            fourth_point = local_intersection_points_path_0[0]

        # 出入角度
        out_yaw = calculate_line_angle(first_point[0], first_point[1], second_point[0], second_point[1], False)  # [rad]
        in_yaw = calculate_line_angle(third_point[0], third_point[1], fourth_point[0], fourth_point[1], False)

        local_path_x, local_path_y, path_yaw, mode, lengths = dubins_path_planning(second_point[0],second_point[1],out_yaw,third_point[0],third_point[1],in_yaw,curvature)

        each_move_distance = 0.05 #单位是米
        each_move_direction = [first_point[0]-second_point[0], first_point[1]-second_point[1]]/np.sqrt(abs(second_point[0]-first_point[0])**2+abs(second_point[1]-first_point[1])**2)

        # 如果移动后的路径曲线仍然有点在多边形之外
        while not is_all_points_inside_poly(local_path_x, local_path_y, local_field_boundary_poly_buffered ):
            # 则继续移动
            moved_local_path_x, moved_local_path_y = move_all_points(local_path_x, local_path_y, each_move_direction, each_move_distance)
            local_path_x = moved_local_path_x
            local_path_y = moved_local_path_y


    return third_point, fourth_point, local_path_x, local_path_y, path_yaw, mode, lengths
    
    """